/*
 * --------------------------------------------------------------------
 * @name UtilitaryNodes.java
 * @author Juan Camilo Ibarra
 *
 * This work is licensed under a Creative
 * Commons Attribution 3.0 License. This means you can to copy, 
 * distribute, transmit and adapt the work, under the following condition:
 * You must attribute the work in the manner specified by the author or licensor.
 *
 * Juan Camilo Ibarra	-	ju-ibarr@uniandes.edu.co
 *
 * IMAGINE research group
 * Universidad de los Andes
 * Oct 3, 2011
 * --------------------------------------------------------------------
 */
package render;

import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileWriter;
import java.io.IOException;
import java.text.SimpleDateFormat;
import java.util.ArrayList;
import java.util.Calendar;
import java.util.Enumeration;
import java.util.Hashtable;

import javax.media.j3d.Appearance;
import javax.media.j3d.BoundingBox;
import javax.media.j3d.BoundingSphere;
import javax.media.j3d.Bounds;
import javax.media.j3d.BranchGroup;
import javax.media.j3d.ColoringAttributes;
import javax.media.j3d.Geometry;
import javax.media.j3d.Group;
import javax.media.j3d.LineArray;
import javax.media.j3d.LineStripArray;
import javax.media.j3d.Material;
import javax.media.j3d.Node;
import javax.media.j3d.PolygonAttributes;
import javax.media.j3d.Shape3D;
import javax.media.j3d.Transform3D;
import javax.media.j3d.TransformGroup;
import javax.vecmath.Color3f;
import javax.vecmath.Point3d;
import javax.vecmath.Point3f;
import javax.vecmath.Vector3d;
import javax.vecmath.Vector3f;

import com.sun.j3d.utils.geometry.Box;
import com.sun.j3d.utils.geometry.Sphere;
import com.sun.j3d.utils.picking.PickTool;

public class UtilitaryMethods {

	public static final String NODE_AXIS = "NODE_AXIS";
	public static final String PICKED_OBJECT_BG_NAME = "PICKED_OBJECT_GEOMETRY";
	public static final String WIND_ROSE_NAME = "WIND_ROSE";

	public static final double EPSILON = 0.00001;
	/**
	 * Creates a bounding Sphere in wireframe for a branchgroup
	 * @param group
	 * @return
	 */
	public static BranchGroup createBoundingSphereNode(BranchGroup group)
	{
		BoundingSphere bounds = (BoundingSphere)group.getBounds();
		Appearance appearance = new Appearance();
		PolygonAttributes pa = new PolygonAttributes();
		pa.setPolygonMode(PolygonAttributes.POLYGON_LINE);
		appearance.setPolygonAttributes(pa);
		Sphere sphere = new Sphere((float)bounds.getRadius(),appearance);
		BranchGroup boundingSphere = new BranchGroup();
		boundingSphere.setCapability(Group.ALLOW_CHILDREN_READ);

		TransformGroup tg = new TransformGroup();
		Transform3D t3D = new Transform3D();
		Point3d center = new Point3d();
		bounds.getCenter(center);
		t3D.setTranslation(new Vector3f((float)center.x, (float)center.y, (float)center.z));
		tg.setTransform(t3D);

		tg.addChild(sphere);

		boundingSphere.addChild(tg);
		System.out.println("Bounding radius: " + bounds.getRadius());
		return boundingSphere;
	}
	
	public static BranchGroup createPlaneGrid(Bounds bounds, int step)
	{
		BranchGroup grid = new BranchGroup();
		NodeCharacteristics.setBranchGroupCapabilities(grid);
		Point3d center = new Point3d();
		double radius = 0;
		if(bounds instanceof BoundingSphere)
		{
			((BoundingSphere) bounds).getCenter(center);
			radius = ((BoundingSphere) bounds).getRadius();
		}
		else if(bounds instanceof BoundingBox)
		{
//			Point3D lower
//			((BoundingBox)bounds).getLower(arg0)
		}
		
		int resolution = (int) (radius * 2.0 / (double)step);
		if(resolution < 1)
		{
			resolution = (int) (radius * 2.0);
		}
		ArrayList<Point3d> points = new ArrayList<Point3d>();
		int[] lineStripCount = new int[(resolution * 2) + 2];
		double lowestx = center.x - radius;
		double lowesty = center.y - radius;
		
		for(int i =0; i < resolution; i++)
		{
			lineStripCount[i] = 2;
			double x = lowestx + (step * (double)i);
			points.add(new Point3d(new double[]{x, center.y - radius, center.z}));
			points.add(new Point3d(new double[]{x, center.y + radius, center.z}));
			
			lineStripCount[resolution + i] = 2;
			double y = lowesty + (step * (double)i);
			points.add(new Point3d(new double[]{center.x - radius, y, center.z}));
			points.add(new Point3d(new double[]{center.x + radius, y, center.z}));	
		}
		
		lineStripCount[(resolution * 2)] = 2;
		points.add(new Point3d(new double[]{center.x + radius, center.y - radius, center.z}));
		points.add(new Point3d(new double[]{center.x + radius, center.y + radius, center.z}));
		
		lineStripCount[(resolution * 2) + 1] = 2;
		points.add(new Point3d(new double[]{center.x - radius, center.y + radius, center.z}));
		points.add(new Point3d(new double[]{center.x + radius, center.y + radius, center.z}));
		
		
		Geometry gridGeometry = new LineStripArray(points.size(), LineArray.COORDINATES, lineStripCount);
		for(int i = 0; i < points.size(); i ++)
		{
			((LineStripArray)gridGeometry).setCoordinate(i, points.get(i));
		}
		Shape3D gridShape = new Shape3D(gridGeometry);
		NodeCharacteristics.setShape3DCapabilites(gridShape);
		grid.addChild(gridShape);
		
		
//		ArrayList<Point3d> axis = new ArrayList<Point3d>();
//		int[] lineStripCountAxis = new int[]{2};
//		axis.add(new Point3d(new double[]{center.x - radius, center.y - radius, 0}));
//		axis.add(new Point3d(new double[]{center.x + radius, center.y - radius, 0}));
//		
//		Geometry axisX = new LineStripArray(axis.size(), LineArray.COORDINATES, lineStripCountAxis);
//		for(int i = 0; i < axis.size(); i ++)
//		{
//			((LineStripArray)gridGeometry).setCoordinate(i, axis.get(i));
//		}
//		
//		
//		Color3f red = new Color3f(1, 0, 0);
//		Appearance xAxisApp = new Appearance();
//		ColoringAttributes ca = new ColoringAttributes(red,ColoringAttributes.SHADE_FLAT);
//		xAxisApp.setColoringAttributes(ca);		
//		
//		
//		
//		Shape3D axisXShape = new Shape3D(axisX, xAxisApp);
//		grid.addChild(axisXShape);
		
		Color3f red = new Color3f(1, 0, 0);
		Color3f green = new Color3f(0, 1, 0);
		Color3f blue = new Color3f(0, 0, 1);
		
		Appearance appRed = new Appearance();
		ColoringAttributes ca = new ColoringAttributes(red,ColoringAttributes.SHADE_FLAT);
		appRed.setColoringAttributes(ca);

		Point3d[] xAxisPts = new Point3d[2];
		xAxisPts[0] = new Point3d(center.x , center.y, center.z);
		xAxisPts[1] = new Point3d(center.x + radius, center.y, center.z);
		LineArray xAxisLine = new LineArray(2, LineArray.COORDINATES);
		xAxisLine.setCoordinates(0, xAxisPts);
		Shape3D xAxis = new Shape3D(xAxisLine, appRed);
		xAxis.setUserData("X Axis");
		grid.addChild(xAxis);

		Appearance appGreen = new Appearance();
		ColoringAttributes cag = new ColoringAttributes(green,
				ColoringAttributes.SHADE_FLAT);
		appGreen.setColoringAttributes(cag);

		Point3d[] yAxisPts = new Point3d[2];
		yAxisPts[0] = new Point3d(center.x, center.y, center.z);
		yAxisPts[1] = new Point3d(center.x, center.y + radius, center.z);
		LineArray yAxisLine = new LineArray(2, LineArray.COORDINATES);
		yAxisLine.setCoordinates(0, yAxisPts);
		Shape3D yAxis = new Shape3D(yAxisLine, appGreen);
		yAxis.setUserData("Y Axis");
		grid.addChild(yAxis);

		Appearance appBlue = new Appearance();
		ColoringAttributes cab = new ColoringAttributes(blue,
				ColoringAttributes.SHADE_FLAT);
		appBlue.setColoringAttributes(cab);

		Point3d[] zAxisPts = new Point3d[2];
		zAxisPts[0] = new Point3d(center.x, center.y, center.z);
		zAxisPts[1] = new Point3d(center.x, center.y, center.z + radius);
		LineArray zAxisLine = new LineArray(2, LineArray.COORDINATES);
		zAxisLine.setCoordinates(0, zAxisPts);
		Shape3D zAxis = new Shape3D(zAxisLine, appBlue);
		zAxis.setUserData("Z Axis");
		grid.addChild(zAxis);
//
//
//		Appearance appGrey = new Appearance();
//		ColoringAttributes caw = new ColoringAttributes(grey,
//				ColoringAttributes.SHADE_FLAT);
//		appGrey.setColoringAttributes(caw);
//		Material material =  new Material();
//
//
//		material.setDiffuseColor(grey);
//		material.setAmbientColor(0.5f, 0.5f, 0.5f);
//		appGrey.setMaterial(material);
//
//		Box box = new Box(0.2f, 0.2f, 0.2f, appGrey);
//		box.setUserData("Box Axis");
//		axis.addChild(box);
//
//		axisBG.addChild(axis);
//		
		return grid;
	}

	/**
	 * Prints the scene graph. The nodes must be readable
	 * @param group
	 * @param indent
	 */
	public static String printGroupInfo(Group group, int indent)
	{
		String groupInfo = "";
		int capabilityToCheck = Node.ENABLE_PICK_REPORTING;
		
		
//		if(group.getUserData() instanceof String)
//		{
//			System.out.println(group.getUserData());
//		}
		@SuppressWarnings("unchecked")
		Enumeration<Node> children = group.getAllChildren();
		String st_ind = "";
		for(int i = 0; i < indent; i++)
		{
			st_ind = st_ind.concat("\t");
		}
		groupInfo = groupInfo.concat(st_ind + group.getClass().getName() + " - " + group.numChildren() + " children" + "\n");
		if(group.getUserData()!= null)
		{
			groupInfo = groupInfo.concat(st_ind + group.getUserData()+ "\n");
		}
		indent++;
		
//		System.out.println(group.getClass().getName() + "***********" + TransformGroup.class.getName());
//		if(group.getClass().getName().equals(TransformGroup.class.getName()))
		if(group instanceof TransformGroup)
		{
//			System.out.println("Is transformGroup!!!");
			Transform3D transform = new Transform3D();
			((TransformGroup) group).getTransform(transform);
			double[] matrix = new double[16];
			transform.get(matrix);
			for(int i = 0; i < matrix.length; i++)
			{
				groupInfo = groupInfo.concat(st_ind + matrix[i] + "\t");
				if(i == 3 || i == 7 || i == 11 || i == 15)
				{
					groupInfo = groupInfo.concat("\n");
				}
			}
		}
		
		while(children.hasMoreElements())
		{
			Node n = children.nextElement();

			if(n instanceof Group)
			{
				groupInfo = groupInfo.concat(printGroupInfo((Group)n, indent)+ "\n");
			}
			else
			{	
				String st_indent = "";
				for(int i = 0; i < indent; i++)
				{
					st_indent = st_indent.concat("\t");
				}
				groupInfo = groupInfo.concat(st_indent + n.getClass().getName() + "\n");
//				Bounds bounds =  n.getBounds();
//				if(bounds instanceof BoundingSphere)
//				{
//					BoundingSphere bsphere = (BoundingSphere)bounds;
//					Point3d center = new Point3d();
//					bsphere.getCenter(center);
//					groupInfo = groupInfo.concat(st_indent + "radius: " + bsphere.getRadius()+ "\n");
//					groupInfo = groupInfo.concat(st_indent + "position: " + center.x + "\t" + center.y + "\t" +center.z + "\n");
//				}
//				else if (bounds instanceof BoundingBox)
//				{
//					BoundingBox bBox = (BoundingBox) bounds;	
//					Point3d lowerPoint = new Point3d();
//					Point3d upperPoint = new Point3d();
//					bBox.getLower(lowerPoint);
//					bBox.getUpper(upperPoint);
//					
//					Point3d center = new Point3d(new double[]{lowerPoint.x + ((upperPoint.x - lowerPoint.x)/2.0), 
//							lowerPoint.y + ((upperPoint.y - lowerPoint.y)/2.0),
//							lowerPoint.z + ((upperPoint.z - lowerPoint.z)/2.0)});
//					
//					groupInfo = groupInfo.concat(st_indent + "center: " + center.x + "\t" + center.y + "\t" +center.z+ "\n");
//					groupInfo = groupInfo.concat(st_indent + "upper: " + upperPoint.x + "\t" + upperPoint.y + "\t" +upperPoint.z + "\n");
//					groupInfo = groupInfo.concat(st_indent + "lower: " + lowerPoint.x + "\t" + lowerPoint.y + "\t" +lowerPoint.z + "\n");
//				}
				
				if(n.getUserData() != null)
				{
//					if(n.getUserData() instanceof VisualObject)
//					{
//						VisualObject vn= (VisualObject)n.getUserData();
//
//						groupInfo = groupInfo.concat(st_indent + vn.getSpatialObjectId() + "-" + vn.getName()+ "\n");
//					}
//					else
//					{
//						groupInfo = groupInfo.concat(st_indent + n.getUserData()+ "\n");
//					}
//					groupInfo = groupInfo.concat(st_indent + n.getCapability(capabilityToCheck)+ "\n");
				}

			}
		}
		return groupInfo;
	}

	@SuppressWarnings({ "rawtypes", "unchecked" })
	public static void centerAndSaveAllShapes3D(BranchGroup group, String folderPath) throws FileNotFoundException, IOException
	{

		Enumeration<Node> children = group.getAllChildren();
		int i = 0;

		File logFile = new File(folderPath + File.separatorChar + "logFile.log");

		FileWriter out = new FileWriter(logFile);
		Calendar currentDate = Calendar.getInstance();
		SimpleDateFormat formatter = new SimpleDateFormat("yyyy/MMM/dd HH:mm:ss");
		out.write(formatter.format(currentDate.getTime()) + "\n");

		ArrayList<String> filesSaved = new ArrayList<String>();
		System.out.println(group.getUserData());
		group.detach();

		Object userData = group.getUserData();
		Hashtable<Shape3D, String> nameTable = new Hashtable<Shape3D, String>();
		if(userData != null)
		{
//			if(userData instanceof VisualObject)
//			{
//				Hashtable<String, Shape3D> namedObjects = (Hashtable<String, Shape3D>) ((VisualObject)userData).getAdditionalInfo();
//				if(namedObjects != null)
//				{
//					Enumeration keys = namedObjects.keys();
//					while( keys.hasMoreElements() ) 
//					{
//						Object aKey = keys.nextElement();
//						Object aValue = namedObjects.get(aKey);
//						nameTable.put((Shape3D)aValue, (String)aKey);
//					}
//				}
//				else
//				{
//					//					nameTable.put();
//				}
//			}
		}

		while(children.hasMoreElements())
		{
			Node n = children.nextElement();

			if(n instanceof Shape3D)
			{
				String fileName = folderPath + File.separatorChar + "shape_" + i + ".obj";
				System.out.println("Trying to save into " + folderPath + " folder");
				System.out.println("Name - > " + nameTable.get(n));

				Bounds bounds = n.getBounds();
				BoundingSphere boundingSphere;
				if (bounds instanceof BoundingSphere)
				{
					boundingSphere = (BoundingSphere) bounds;
				}
				else
				{
					boundingSphere = new BoundingSphere(bounds);
				}

				Point3d center = new Point3d();
				boundingSphere.getCenter(center);
				Vector3d translation = new Vector3d(new double[]{-center.x, -center.y, -center.z});

				BranchGroup bg = new BranchGroup();
				Transform3D t3D = new Transform3D();
				t3D.setTranslation(translation);
				TransformGroup tg = new TransformGroup(t3D);

				tg.addChild(n.cloneNode(false));
				bg.addChild(tg);

				ObjWriter objWriter = new ObjWriter(fileName);
				objWriter.writeNode(bg);
				objWriter.close();
				System.out.println("Saved: \nfile: " + fileName + " - position: " + center.toString());
				filesSaved.add("file." + i + "=" + fileName);
				filesSaved.add("file." + i + ".posx=" + center.x);
				filesSaved.add("file." + i + ".posy=" + center.y);
				filesSaved.add("file." + i + ".posz=" + center.z);
				i++;
			}
			else if(n instanceof TransformGroup)
			{
				String nodeName = nameTable.get(((TransformGroup) n).getChild(0));
				String fileName = folderPath + File.separatorChar + nodeName + ".obj";
				System.out.println("Trying to save into " + folderPath + " folder");

				Bounds bounds = n.getBounds();
				BoundingSphere boundingSphere;
				if (bounds instanceof BoundingSphere)
				{
					boundingSphere = (BoundingSphere) bounds;
				}
				else
				{
					boundingSphere = new BoundingSphere(bounds);
				}

				Point3d center = new Point3d();
				boundingSphere.getCenter(center);
				Vector3d translation = new Vector3d(new double[]{-center.x, -center.y, -center.z});

				BranchGroup bg = new BranchGroup();
				Transform3D t3D = new Transform3D();
				t3D.setTranslation(translation);
				((TransformGroup) n).setTransform(t3D);

				bg.addChild(n.cloneTree());


				ObjWriter objWriter = new ObjWriter(fileName);
				objWriter.writeNode(bg, nodeName);
				objWriter.close();
				System.out.println("Saved: \nfile: " + fileName + " - position: " + center.toString());
				filesSaved.add("file." + i + "=" + fileName);
				filesSaved.add("file." + i + ".posx=" + center.x);
				filesSaved.add("file." + i + ".posy=" + center.y);
				filesSaved.add("file." + i + ".posz=" + center.z);
				i++;
			}
		}
		out.write("file.count=" + i + "\n");
		for(String st : filesSaved)
		{
			out.write(st + "\n");
		}
		out.close();
		System.exit(0);
	}


	
	@SuppressWarnings("unchecked")
	public static void enablePicking(Node node) {

		//		Object userData = node.getUserData();
		//		if(userData instanceof VisualObject)
		//		{
		//			System.out.println(((VisualObject)userData).getName());
		//		}
		node.setPickable(true);
		node.setCapability(Node.ENABLE_PICK_REPORTING);
		try {
			Group group = (Group) node;
			for (Enumeration e = group.getAllChildren(); e.hasMoreElements();) {
				enablePicking((Node)e.nextElement());
			}
		}
		catch(ClassCastException e) {
			// if not a group node, there are no children so ignore exception
		}
		try {
			Shape3D shape = (Shape3D) node;

			PickTool.setCapabilities(node, PickTool.INTERSECT_FULL);
			Enumeration e = shape.getAllGeometries();
			while (e.hasMoreElements()) {
				Geometry g = (Geometry)e.nextElement();
				g.setCapability(Geometry.ALLOW_INTERSECT);
			}
		}
		catch(ClassCastException e) {
			// not a Shape3D node ignore exception
		}
	}

	/**
	 * Creates an XYZ axis in the scene
	 * @return
	 */
	public static BranchGroup createAxis(double scale)
	{		
		BranchGroup axisBG = new BranchGroup();
		axisBG.setCapability(Group.ALLOW_CHILDREN_READ);
		TransformGroup axis = new TransformGroup();
		Transform3D transform = new Transform3D();
		transform.setScale(scale);
		axis.setTransform(transform);


		axis.setUserData(NODE_AXIS);
		axis.setCapability(Group.ALLOW_CHILDREN_READ);
		Color3f red = new Color3f(1, 0, 0);
		Color3f green = new Color3f(0, 1, 0);
		Color3f blue = new Color3f(0, 0, 1);
		Color3f grey = new Color3f(0.8f,0.8f,0.8f);
		//		Color3f white = new Color3f(1,1,1);

		Appearance appRed = new Appearance();
		ColoringAttributes ca = new ColoringAttributes(red,
				ColoringAttributes.SHADE_FLAT);
		appRed.setColoringAttributes(ca);

		Point3f[] xAxisPts = new Point3f[2];
		xAxisPts[0] = new Point3f(0.0f, 0.0f, 0.0f);
		xAxisPts[1] = new Point3f(1.0f, 0.0f, 0.0f);
		LineArray xAxisLine = new LineArray(2, LineArray.COORDINATES);
		xAxisLine.setCoordinates(0, xAxisPts);
		Shape3D xAxis = new Shape3D(xAxisLine, appRed);
		xAxis.setUserData("X Axis");
		axis.addChild(xAxis);

		Appearance appGreen = new Appearance();
		ColoringAttributes cag = new ColoringAttributes(green,
				ColoringAttributes.SHADE_FLAT);
		appGreen.setColoringAttributes(cag);

		Point3f[] yAxisPts = new Point3f[2];
		yAxisPts[0] = new Point3f(0.0f, 0.0f, 0.0f);
		yAxisPts[1] = new Point3f(0.0f, 1.0f, 0.0f);
		LineArray yAxisLine = new LineArray(2, LineArray.COORDINATES);
		yAxisLine.setCoordinates(0, yAxisPts);
		Shape3D yAxis = new Shape3D(yAxisLine, appGreen);
		yAxis.setUserData("Y Axis");
		axis.addChild(yAxis);

		Appearance appBlue = new Appearance();
		ColoringAttributes cab = new ColoringAttributes(blue,
				ColoringAttributes.SHADE_FLAT);
		appBlue.setColoringAttributes(cab);

		Point3f[] zAxisPts = new Point3f[2];
		zAxisPts[0] = new Point3f(0.0f, 0.0f, 0.0f);
		zAxisPts[1] = new Point3f(0.0f, 0.0f, 1.0f);
		LineArray zAxisLine = new LineArray(2, LineArray.COORDINATES);
		zAxisLine.setCoordinates(0, zAxisPts);
		Shape3D zAxis = new Shape3D(zAxisLine, appBlue);
		zAxis.setUserData("Z Axis");
		axis.addChild(zAxis);


		Appearance appGrey = new Appearance();
		ColoringAttributes caw = new ColoringAttributes(grey,
				ColoringAttributes.SHADE_FLAT);
		appGrey.setColoringAttributes(caw);
		Material material =  new Material();


		material.setDiffuseColor(grey);
		material.setAmbientColor(0.5f, 0.5f, 0.5f);
		appGrey.setMaterial(material);

		Box box = new Box(0.2f, 0.2f, 0.2f, appGrey);
		box.setUserData("Box Axis");
		axis.addChild(box);

		axisBG.addChild(axis);
		return axisBG;
	}

	/**
	 * Check if the point is inside the triangle defined for the other 3 points
	 * @param point
	 * @param p1
	 * @param p2
	 * @param p3
	 * @return
	 */
	public static boolean pointInTriangle(Point3d point, Point3d p1, Point3d p2, Point3d p3)
	{
		Vector3d v0 = UtilitaryMethods.createVector(p2, p3); 
		Vector3d v1 = UtilitaryMethods.createVector(p2, p1);
		Vector3d v2 = UtilitaryMethods.createVector(p2, point);

		double dot00 = v0.dot(v0);
		double dot01 = v0.dot(v1);
		double dot02 = v0.dot(v2);
		double dot11 = v1.dot(v1);
		double dot12 = v1.dot(v2);



		double invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
		double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
		double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
		return (u > 0) && (v > 0) && (u + v < 1);
	}

	private static Vector3d createVector(Point3d p1, Point3d p2)
	{
		return new Vector3d(p2.x - p1.x, p2.y - p1.y, p2.z - p1.z);
	}

	public static Point3d getPrevOf(ArrayList<Point3d> pointList, int index)
	{
		if(index == 0)
		{
			return pointList.get(pointList.size() - 1);
		}
		else
		{
			return pointList.get(index - 1);
		}
	}

	public static Point3d getNextOf(ArrayList<Point3d> pointList, int index)
	{
		if(index == pointList.size() - 1)
		{
			return pointList.get(0);
		}
		else
		{
			return pointList.get(index + 1);
		}

	}

	public static double perpDotProduct(Vector3d p1, Vector3d p2)
	{
		return ((p1.y * p2.z) + (p1.z * p2.x) + (p1.x * p2.y))-((p1.z * p2.y) + (p1.x * p2.z) + (p1.y * p2.x));
	}


	public static double angleBetween(Point3d p1, Point3d p2, Point3d p3)
	{
		double x = p1.x - p2.x;
		double y = p1.y - p2.y;
		double z = p1.z - p2.z;

		double x_ = p3.x - p2.x;
		double y_ = p3.y - p2.y;
		double z_ = p3.z - p2.z;

		Vector3d p1_local = new Vector3d(x, y, z);
		Vector3d p2_local = new Vector3d(x_, y_, z_);

		double dotProduct = p1_local.dot(p2_local);
		double perpdotProduct = UtilitaryMethods.perpDotProduct(p1_local, p2_local);
		Vector3d crossV = new Vector3d();
		crossV.cross(p1_local, p2_local);
		Point3d zeroV = new Point3d(0,0,0);
		Point3d crossVP = new Point3d(crossV.x, crossV.y, crossV.z);
		double lengthV = crossVP.distance(zeroV);
		double angle = Math.atan2(lengthV, dotProduct);

		if(perpdotProduct >= 0)
		{
			angle = -angle;
		}



		angle *= (180 / Math.PI);
		if(angle < EPSILON && angle > -EPSILON)
		{
			angle = 0;
		}
		return angle;
	}

	/**
	 * Finds the intersection point between two segments
	 * http://softsurfer.com/Archive/algorithm_0104/algorithm_0104B.htm#intersect2D_SegSeg()
	 * @param p1
	 * @param p2
	 * @param p3
	 * @param p4
	 * @param intX
	 * @param intY
	 * @return
	 */
	public static int Intersect2DSegments(Point3d p1, Point3d p2, Point3d p3, Point3d p4, double intX, double intY)
	{
		Vector3d u = createVector(p1, p2);
		Vector3d v = createVector(p3, p4);
		Vector3d w = createVector(p3, p1);
		double D = perpDotProduct(u,v);

		// test if they are parallel (includes either being a point)
		if (Math.abs(D) < EPSILON) 
		{          // S1 and S2 are parallel
			if (perpDotProduct(u,w) != 0 || perpDotProduct(v,w) != 0) 
			{
				return 0;                   // they are NOT collinear
			}
			// they are collinear or degenerate
			// check if they are degenerate points
			//			double du = u.dot(u);
			//			double dv = v.dot(v);

			// they are collinear segments - get overlap (or not)
			double t0, t1;                   // endpoints of S1 in eqn for S2
			Vector3d w2 = createVector(p2, p3);
			if (v.x != 0) 
			{
				t0 = w.x / v.x;
				t1 = w2.x / v.x;
			}
			else 
			{
				t0 = w.y / v.y;
				t1 = w2.y / v.y;
			}
			if (t0 > t1) 
			{                  // must have t0 smaller than t1
				double t=t0; t0=t1; t1=t;    // swap if not
			}
			if (t0 > 1 || t1 < 0) {
				return 0;     // NO overlap
			}
			t0 = t0<0? 0 : t0;              // clip to min 0
			t1 = t1>1? 1 : t1;              // clip to max 1
			if (t0 == t1) {                 // intersect is a point
				double x = p3.x + (t0 * v.x);
				double y = p3.y + (t0 * v.y);
				intX = x;
				intY = y;
				return 1;
			}
		}
		// the segments are skew and may intersect in a point
		// get the intersect parameter for S1
		double sI = perpDotProduct(v,w) / D;
		if (sI < 0 || sI > 1)               // no intersect with S1
			return 0;

		// get the intersect parameter for S2
		double     tI = perpDotProduct(u,w) / D;
		if (tI < 0 || tI > 1)               // no intersect with S2
			return 0;

		// compute S1 intersect point
		double x2 = p1.x + (sI * u.x);
		double y2 = p1.y + (sI * u.y);
		intX = x2;
		intY = y2;
		return 1;
	}


	


}

